//
// Created by zzy on 22-5-22.
//

#ifndef TOGETHER_ANGLESOLVER_H
#define TOGETHER_ANGLESOLVER_H
#ifndef ANGLESOLVER_H
#define ANGLESOLVER_H

#include<opencv2/calib3d.hpp>
#include<opencv2/opencv.hpp>
#include<vector>
#include"ArmorDetectorT/DataType.hpp"

//armor size
#define BIG_ARMOR_LEN 217.5
#define BIG_ARMOR_WID 49.5
#define SMALL_ARMOR_LEN 137.2
#define SMALL_ARMOR_WID 54.1

namespace hnurm
{

    class AngleSolver
    {
    public:

        AngleSolver()=default;
        ~AngleSolver()=default;

        /**
         * @brief
         *
         * @return true
         * @return false
         */
        bool Init();


        /**
         * @brief
         *
         * @param target
         * @param distance
         * @param pitch
         * @param yaw
         */
        void Solve(TargetInfo& target,float& distance,float& pitch,float& yaw);


        /**
         * @brief get the world coordinates of target,in order to predict movement
         *        no paremeter is required,using _translation_vect
         */
        void CoordinatesTrans();


        //overload b of realsense bifocal camera
        //just return the depth,that's nice!
        //bool SolveDistance();

    private:
        cv::Mat _camera_matrix;       //Camera Matrix from Camera calibration
        cv::Mat _distortion_matrix;   //Distortion Matrix from Camera calibration

        std::vector<cv::Point3f> _big_armor_real;
        std::vector<cv::Point3f> _small_armor_real;

        std::vector<cv::Point3d> _target_world_coordinate;
        cv::Mat _rotation_vect;
        cv::Mat _translation_vect;

    };//anglesolver

} // namespace hnurm

#endif // ANGLESOLVER_H

#endif //TOGETHER_ANGLESOLVER_H
